#include "Maths.h"

using namespace maths;

const mFloat g_Zero = 0.0f;
const mFloat g_One  = 1.0f;

CMatrix4x4f::CMatrix4x4f()
{
	__asm
	{
		xorps xmm0, xmm0
		mov eax, dword ptr this
		movss xmm0, g_One
		movaps xmmword ptr [eax     ], xmm0
		pshufd xmm0, xmm0, 10010011b
		movaps xmmword ptr [eax + 16], xmm0
		pshufd xmm0, xmm0, 10010011b
		movaps xmmword ptr [eax + 32], xmm0
		pshufd xmm0, xmm0, 10010011b
		movaps xmmword ptr [eax + 48], xmm0
	}
}

CMatrix4x4f::CMatrix4x4f(float a_M00, float a_M01, float a_M02, float a_M03, 
					   float a_M10, float a_M11, float a_M12, float a_M13, 
					   float a_M20, float a_M21, float a_M22, float a_M23, 
					   float a_M30, float a_M31, float a_M32, float a_M33)
{
	m_Data[ 0] = a_M00; m_Data[ 1] = a_M01; m_Data[ 2] = a_M02; m_Data[ 3] = a_M03;
	m_Data[ 4] = a_M10; m_Data[ 5] = a_M11; m_Data[ 6] = a_M12; m_Data[ 7] = a_M13;
	m_Data[ 8] = a_M20; m_Data[ 9] = a_M21; m_Data[10] = a_M22; m_Data[11] = a_M23;
	m_Data[12] = a_M30; m_Data[13] = a_M31; m_Data[14] = a_M32; m_Data[15] = a_M33;
}

CMatrix4x4f::CMatrix4x4f(const CMatrix4x4f& a_M)
{
	__asm
	{
		mov eax, dword ptr a_M
		movaps xmm0, xmmword ptr [eax     ]
		movaps xmm1, xmmword ptr [eax + 16]
		movaps xmm2, xmmword ptr [eax + 32]
		movaps xmm3, xmmword ptr [eax + 48]
		mov eax, dword ptr this
		movaps xmmword ptr [eax     ], xmm0
		movaps xmmword ptr [eax + 16], xmm1
		movaps xmmword ptr [eax + 32], xmm2
		movaps xmmword ptr [eax + 48], xmm3
	}
}

float& CMatrix4x4f :: operator [](int a_Index) 
{
	return m_Data[a_Index];
}

CMatrix4x4f& CMatrix4x4f :: operator  = (const CMatrix4x4f& a_M)
{
	__asm
	{
		mov eax, dword ptr a_M
		movaps xmm0, xmmword ptr [eax     ]
		movaps xmm1, xmmword ptr [eax + 16]
		movaps xmm2, xmmword ptr [eax + 32]
		movaps xmm3, xmmword ptr [eax + 48]
		mov eax, dword ptr this
		movaps xmmword ptr [eax     ], xmm0
		movaps xmmword ptr [eax + 16], xmm1
		movaps xmmword ptr [eax + 32], xmm2
		movaps xmmword ptr [eax + 48], xmm3
	}
	return *this;
}

CMatrix4x4f& CMatrix4x4f :: operator += (const CMatrix4x4f& a_M)
{
	__asm
	{
		mov eax, dword ptr a_M
		movaps xmm0, xmmword ptr [eax     ]
		movaps xmm1, xmmword ptr [eax + 16]
		movaps xmm2, xmmword ptr [eax + 32]
		movaps xmm3, xmmword ptr [eax + 48]

		mov eax, dword ptr this
		movaps xmm4, xmmword ptr [eax     ]
		movaps xmm5, xmmword ptr [eax + 16]
		movaps xmm6, xmmword ptr [eax + 32]
		movaps xmm7, xmmword ptr [eax + 48]

		addps xmm0, xmm4
		addps xmm1, xmm5
		addps xmm2, xmm6
		addps xmm3, xmm7

		movaps xmmword ptr [eax     ], xmm0
		movaps xmmword ptr [eax + 16], xmm1
		movaps xmmword ptr [eax + 32], xmm2
		movaps xmmword ptr [eax + 48], xmm3
	}
	return *this;
}

CMatrix4x4f& CMatrix4x4f :: operator -= (const CMatrix4x4f& a_M)
{
	__asm
	{
		mov eax, dword ptr a_M
		movaps xmm0, xmmword ptr [eax     ]
		movaps xmm1, xmmword ptr [eax + 16]
		movaps xmm2, xmmword ptr [eax + 32]
		movaps xmm3, xmmword ptr [eax + 48]

		mov eax, dword ptr this
		movaps xmm4, xmmword ptr [eax     ]
		movaps xmm5, xmmword ptr [eax + 16]
		movaps xmm6, xmmword ptr [eax + 32]
		movaps xmm7, xmmword ptr [eax + 48]

		subps xmm0, xmm4
		subps xmm1, xmm5
		subps xmm2, xmm6
		subps xmm3, xmm7

		movaps xmmword ptr [eax     ], xmm0
		movaps xmmword ptr [eax + 16], xmm1
		movaps xmmword ptr [eax + 32], xmm2
		movaps xmmword ptr [eax + 48], xmm3
	}
	return *this;
}

CMatrix4x4f& CMatrix4x4f :: operator *= (const CMatrix4x4f& a_M)
{
	__asm
	{
		mov eax, dword ptr a_M
		movaps xmm4, xmmword ptr [eax     ] //xmm4 = d c b a
		movaps xmm5, xmmword ptr [eax + 16] //xmm5 = l k n m
		movaps xmm6, xmmword ptr [eax + 32] //xmm6 = s r q p
		movaps xmm7, xmmword ptr [eax + 48] //xmm7 = t z y x

		mov eax, dword ptr this
		movaps xmm3, xmmword ptr [eax     ] //xmm3 = 4 3 2 1
		pshufd xmm0, xmm3, 00000000b        //xmm0 = 1 1 1 1
		pshufd xmm1, xmm3, 01010101b        //xmm1 = 2 2 2 2
		pshufd xmm2, xmm3, 10101010b        //xmm2 = 3 3 3 3
		pshufd xmm3, xmm3, 11111111b        //xmm3 = 4 4 4 4

		mulps  xmm0, xmm4                   //xmm0 = 1d 1c 1b 1a
		mulps  xmm1, xmm5                   //xmm1 = 2l 2k 2n 2m
		mulps  xmm2, xmm6                   //xmm2 = 3s 3r 3q 3p
		mulps  xmm3, xmm7                   //xmm3 = 4t 4z 4y 4x

		addps  xmm0, xmm1
		addps  xmm0, xmm2
		addps  xmm0, xmm3                   //xmm0 = 1d+2l+3c+4t 1c+2k+3r+4z 1b+2n+3q+4y 1a+2m+3p+4x

		movaps xmmword ptr [eax     ], xmm0


		movaps xmm3, xmmword ptr [eax + 16]
		pshufd xmm0, xmm3, 00000000b
		pshufd xmm1, xmm3, 01010101b
		pshufd xmm2, xmm3, 10101010b
		pshufd xmm3, xmm3, 11111111b

		mulps  xmm0, xmm4
		mulps  xmm1, xmm5
		mulps  xmm2, xmm6
		mulps  xmm3, xmm7

		addps  xmm0, xmm1
		addps  xmm0, xmm2
		addps  xmm0, xmm3

		movaps xmmword ptr [eax + 16], xmm0


		movaps xmm3, xmmword ptr [eax + 32]
		pshufd xmm0, xmm3, 00000000b
		pshufd xmm1, xmm3, 01010101b
		pshufd xmm2, xmm3, 10101010b
		pshufd xmm3, xmm3, 11111111b

		mulps  xmm0, xmm4
		mulps  xmm1, xmm5
		mulps  xmm2, xmm6
		mulps  xmm3, xmm7

		addps  xmm0, xmm1
		addps  xmm0, xmm2
		addps  xmm0, xmm3

		movaps xmmword ptr [eax + 32], xmm0


		movaps xmm3, xmmword ptr [eax + 48]
		pshufd xmm0, xmm3, 00000000b
		pshufd xmm1, xmm3, 01010101b
		pshufd xmm2, xmm3, 10101010b
		pshufd xmm3, xmm3, 11111111b

		mulps  xmm0, xmm4
		mulps  xmm1, xmm5
		mulps  xmm2, xmm6
		mulps  xmm3, xmm7

		addps  xmm0, xmm1
		addps  xmm0, xmm2
		addps  xmm0, xmm3

		movaps xmmword ptr [eax + 48], xmm0
	}
	return *this;
}

CMatrix4x4f& CMatrix4x4f :: operator *= (const float a_K)
{
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax     ]
		movaps xmm1, xmmword ptr [eax + 16]
		movaps xmm2, xmmword ptr [eax + 32]
		movaps xmm3, xmmword ptr [eax + 48]
		movss  xmm4, a_K
		pshufd xmm4, xmm4, 00h
		mulps  xmm0, xmm4
		mulps  xmm1, xmm4
		mulps  xmm2, xmm4
		mulps  xmm3, xmm4
		movaps xmmword ptr [eax     ], xmm0
		movaps xmmword ptr [eax + 16], xmm0
		movaps xmmword ptr [eax + 32], xmm0
		movaps xmmword ptr [eax + 48], xmm0
	}
	return *this;
}

CMatrix4x4f& CMatrix4x4f :: operator /= (const float a_K)
{
	__asm
	{
		mov eax, dword ptr this
			movaps xmm0, xmmword ptr [eax     ]
		movaps xmm1, xmmword ptr [eax + 16]
		movaps xmm2, xmmword ptr [eax + 32]
		movaps xmm3, xmmword ptr [eax + 48]
		movss  xmm4, a_K
		pshufd xmm4, xmm4, 00h
		divps  xmm0, xmm4
		divps  xmm1, xmm4
		divps  xmm2, xmm4
		divps  xmm3, xmm4
		movaps xmmword ptr [eax     ], xmm0
		movaps xmmword ptr [eax + 16], xmm0
		movaps xmmword ptr [eax + 32], xmm0
		movaps xmmword ptr [eax + 48], xmm0
	}
	return *this;
}

bool CMatrix4x4f :: operator != (const CMatrix4x4f& a_M)
{
	return (
		(fabs(m_Data[ 0] - a_M.m_Data[ 0]) > _FLOAT_EPS_) ||
		(fabs(m_Data[ 1] - a_M.m_Data[ 1]) > _FLOAT_EPS_) ||
		(fabs(m_Data[ 2] - a_M.m_Data[ 2]) > _FLOAT_EPS_) ||
		(fabs(m_Data[ 3] - a_M.m_Data[ 3]) > _FLOAT_EPS_) ||
		(fabs(m_Data[ 4] - a_M.m_Data[ 4]) > _FLOAT_EPS_) ||
		(fabs(m_Data[ 5] - a_M.m_Data[ 5]) > _FLOAT_EPS_) ||
		(fabs(m_Data[ 6] - a_M.m_Data[ 6]) > _FLOAT_EPS_) ||
		(fabs(m_Data[ 7] - a_M.m_Data[ 7]) > _FLOAT_EPS_) ||
		(fabs(m_Data[ 8] - a_M.m_Data[ 8]) > _FLOAT_EPS_) ||
		(fabs(m_Data[ 9] - a_M.m_Data[ 9]) > _FLOAT_EPS_) ||
		(fabs(m_Data[10] - a_M.m_Data[10]) > _FLOAT_EPS_) ||
		(fabs(m_Data[11] - a_M.m_Data[11]) > _FLOAT_EPS_) ||
		(fabs(m_Data[12] - a_M.m_Data[12]) > _FLOAT_EPS_) ||
		(fabs(m_Data[13] - a_M.m_Data[13]) > _FLOAT_EPS_) ||
		(fabs(m_Data[14] - a_M.m_Data[14]) > _FLOAT_EPS_) ||
		(fabs(m_Data[15] - a_M.m_Data[15]) > _FLOAT_EPS_));
}


bool CMatrix4x4f :: operator == (const CMatrix4x4f& a_M)
{
	return (
		(fabs(m_Data[ 0] - a_M.m_Data[ 0]) < _FLOAT_EPS_) &&
		(fabs(m_Data[ 1] - a_M.m_Data[ 1]) < _FLOAT_EPS_) &&
		(fabs(m_Data[ 2] - a_M.m_Data[ 2]) < _FLOAT_EPS_) &&
		(fabs(m_Data[ 3] - a_M.m_Data[ 3]) < _FLOAT_EPS_) &&
		(fabs(m_Data[ 4] - a_M.m_Data[ 4]) < _FLOAT_EPS_) &&
		(fabs(m_Data[ 5] - a_M.m_Data[ 5]) < _FLOAT_EPS_) &&
		(fabs(m_Data[ 6] - a_M.m_Data[ 6]) < _FLOAT_EPS_) &&
		(fabs(m_Data[ 7] - a_M.m_Data[ 7]) < _FLOAT_EPS_) &&
		(fabs(m_Data[ 8] - a_M.m_Data[ 8]) < _FLOAT_EPS_) &&
		(fabs(m_Data[ 9] - a_M.m_Data[ 9]) < _FLOAT_EPS_) &&
		(fabs(m_Data[10] - a_M.m_Data[10]) < _FLOAT_EPS_) &&
		(fabs(m_Data[11] - a_M.m_Data[11]) < _FLOAT_EPS_) &&
		(fabs(m_Data[12] - a_M.m_Data[12]) < _FLOAT_EPS_) &&
		(fabs(m_Data[13] - a_M.m_Data[13]) < _FLOAT_EPS_) &&
		(fabs(m_Data[14] - a_M.m_Data[14]) < _FLOAT_EPS_) &&
		(fabs(m_Data[15] - a_M.m_Data[15]) < _FLOAT_EPS_));
}

CMatrix4x4f CMatrix4x4f :: operator + (const CMatrix4x4f& a_M) const
{
	CMatrix4x4f l_Result;
	__asm
	{
		mov eax, dword ptr a_M
		movaps xmm0, xmmword ptr [eax     ]
		movaps xmm1, xmmword ptr [eax + 16]
		movaps xmm2, xmmword ptr [eax + 32]
		movaps xmm3, xmmword ptr [eax + 48]

		mov eax, dword ptr this
		movaps xmm4, xmmword ptr [eax     ]
		movaps xmm5, xmmword ptr [eax + 16]
		movaps xmm6, xmmword ptr [eax + 32]
		movaps xmm7, xmmword ptr [eax + 48]

		addps xmm0, xmm4
		addps xmm1, xmm5
		addps xmm2, xmm6
		addps xmm3, xmm7

		movaps xmmword ptr [l_Result     ], xmm0
		movaps xmmword ptr [l_Result + 16], xmm1
		movaps xmmword ptr [l_Result + 32], xmm2
		movaps xmmword ptr [l_Result + 48], xmm3
	}
	return l_Result;
}

CMatrix4x4f CMatrix4x4f :: operator - (const CMatrix4x4f& a_M) const
{
	CMatrix4x4f l_Result;
	__asm
	{
		mov eax, dword ptr a_M
		movaps xmm0, xmmword ptr [eax     ]
		movaps xmm1, xmmword ptr [eax + 16]
		movaps xmm2, xmmword ptr [eax + 32]
		movaps xmm3, xmmword ptr [eax + 48]

		mov eax, dword ptr this
		movaps xmm4, xmmword ptr [eax     ]
		movaps xmm5, xmmword ptr [eax + 16]
		movaps xmm6, xmmword ptr [eax + 32]
		movaps xmm7, xmmword ptr [eax + 48]

		subps xmm0, xmm4
		subps xmm1, xmm5
		subps xmm2, xmm6
		subps xmm3, xmm7

		movaps xmmword ptr [l_Result     ], xmm0
		movaps xmmword ptr [l_Result + 16], xmm1
		movaps xmmword ptr [l_Result + 32], xmm2
		movaps xmmword ptr [l_Result + 48], xmm3
	}
	return l_Result;
}

CMatrix4x4f CMatrix4x4f :: operator * (const CMatrix4x4f& a_M) const
{
	CMatrix4x4f l_Result;
	__asm
	{
		mov eax, dword ptr a_M
		movaps xmm4, xmmword ptr [eax     ]
		movaps xmm5, xmmword ptr [eax + 16]
		movaps xmm6, xmmword ptr [eax + 32]
		movaps xmm7, xmmword ptr [eax + 48]

		mov eax, dword ptr this
		movaps xmm3, xmmword ptr [eax     ]
		pshufd xmm0, xmm3, 00000000b
		pshufd xmm1, xmm3, 01010101b
		pshufd xmm2, xmm3, 10101010b
		pshufd xmm3, xmm3, 11111111b

		mulps  xmm0, xmm4
		mulps  xmm1, xmm5
		mulps  xmm2, xmm6
		mulps  xmm3, xmm7

		addps  xmm0, xmm1
		addps  xmm0, xmm2
		addps  xmm0, xmm3

		movaps xmmword ptr [l_Result     ], xmm0


		movaps xmm3, xmmword ptr [eax + 16]
		pshufd xmm0, xmm3, 00000000b
		pshufd xmm1, xmm3, 01010101b
		pshufd xmm2, xmm3, 10101010b
		pshufd xmm3, xmm3, 11111111b

		mulps  xmm0, xmm4
		mulps  xmm1, xmm5
		mulps  xmm2, xmm6
		mulps  xmm3, xmm7

		addps  xmm0, xmm1
		addps  xmm0, xmm2
		addps  xmm0, xmm3

		movaps xmmword ptr [l_Result + 16], xmm0


		movaps xmm3, xmmword ptr [eax + 32]
		pshufd xmm0, xmm3, 00000000b
		pshufd xmm1, xmm3, 01010101b
		pshufd xmm2, xmm3, 10101010b
		pshufd xmm3, xmm3, 11111111b

		mulps  xmm0, xmm4
		mulps  xmm1, xmm5
		mulps  xmm2, xmm6
		mulps  xmm3, xmm7

		addps  xmm0, xmm1
		addps  xmm0, xmm2
		addps  xmm0, xmm3

		movaps xmmword ptr [l_Result + 32], xmm0


		movaps xmm3, xmmword ptr [eax + 48]
		pshufd xmm0, xmm3, 00000000b
		pshufd xmm1, xmm3, 01010101b
		pshufd xmm2, xmm3, 10101010b
		pshufd xmm3, xmm3, 11111111b

		mulps  xmm0, xmm4
		mulps  xmm1, xmm5
		mulps  xmm2, xmm6
		mulps  xmm3, xmm7

		addps  xmm0, xmm1
		addps  xmm0, xmm2
		addps  xmm0, xmm3

		movaps xmmword ptr [l_Result + 48], xmm0
	}
	return l_Result;
}

CMatrix4x4f CMatrix4x4f :: operator * (const float a_K) const
{
	CMatrix4x4f l_Result;
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax     ]
		movaps xmm1, xmmword ptr [eax + 16]
		movaps xmm2, xmmword ptr [eax + 32]
		movaps xmm3, xmmword ptr [eax + 48]
		movss  xmm4, a_K
		pshufd xmm4, xmm4, 00h
		mulps  xmm0, xmm4
		mulps  xmm1, xmm4
		mulps  xmm2, xmm4
		mulps  xmm3, xmm4
		movaps xmmword ptr [l_Result     ], xmm0
		movaps xmmword ptr [l_Result + 16], xmm0
		movaps xmmword ptr [l_Result + 32], xmm0
		movaps xmmword ptr [l_Result + 48], xmm0
	}
	return l_Result;
}

CMatrix4x4f CMatrix4x4f :: operator / (const float a_K) const
{
	CMatrix4x4f l_Result;
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax     ]
		movaps xmm1, xmmword ptr [eax + 16]
		movaps xmm2, xmmword ptr [eax + 32]
		movaps xmm3, xmmword ptr [eax + 48]
		movss  xmm4, a_K
		pshufd xmm4, xmm4, 00h
		divps  xmm0, xmm4
		divps  xmm1, xmm4
		divps  xmm2, xmm4
		divps  xmm3, xmm4
		movaps xmmword ptr [l_Result     ], xmm0
		movaps xmmword ptr [l_Result + 16], xmm0
		movaps xmmword ptr [l_Result + 32], xmm0
		movaps xmmword ptr [l_Result + 48], xmm0
	}
	return l_Result;
}

float CMatrix4x4f :: operator ! () const
{
	float l_Result;
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax     ] //xmm0 = a b c d
		movaps xmm1, xmmword ptr [eax + 16] //xmm1 = k l m n
		movaps xmm2, xmmword ptr [eax + 32] //xmm2 = p q r s
		movaps xmm3, xmmword ptr [eax + 48] //xmm3 = x y z t

		pshufd xmm4, xmm1, 10110001b        //xmm4 = l k n m
		pshufd xmm5, xmm3, 10110001b        //xmm5 = y x t z

		mulps  xmm4, xmm0                   //xmm4 = al bk cn dm
		mulps  xmm5, xmm2                   //xmm5 = py qx rt sz

		pshufd xmm7, xmm5, 01001110b        //xmm7 = rt sz py qx
		mulps  xmm7, xmm4                   //xmm7 = alrt bksz cnpy dmqx

		pshufd xmm6, xmm5, 00011011b        //xmm6 = sz rt qx py
		mulps  xmm6, xmm4                   //xmm6 = alsz bkrt cnqx dmpy
		subps  xmm7, xmm6


		pshufd xmm4, xmm1, 01001110b        //xmm4 = m n k l
		pshufd xmm5, xmm3, 01001110b        //xmm5 = z t x y

		mulps  xmm4, xmm0                   //xmm4 = am bn ck dl
		mulps  xmm5, xmm2                   //xmm5 = pz qt rx sy

		pshufd xmm6, xmm5, 10110001b        //xmm6 = qt pz sy rx
		mulps  xmm6, xmm4                   //xmm6 = amqt bnpz cksy dlrx
		subps  xmm7, xmm6

		pshufd xmm6, xmm5, 00011011b        //xmm6 = sy rx qt pz
		mulps  xmm6, xmm4                   //xmm6 = amsy bnrx ckqt dlpz
		addps  xmm7, xmm6


		pshufd xmm4, xmm1, 00011011b        //xmm4 = n m l k
		pshufd xmm5, xmm3, 00011011b        //xmm5 = t z y x

		mulps  xmm4, xmm0                   //xmm4 = an bm cl dk
		mulps  xmm5, xmm2                   //xmm5 = pt qz ry sx

		pshufd xmm6, xmm5, 01001110b        //xmm6 = ry sx pt qz
		mulps  xmm6, xmm4                   //xmm6 = anry bmsx clpt dkqz
		subps  xmm7, xmm6

		pshufd xmm6, xmm5, 10110001b        //xmm6 = qz pt sx ry
		mulps  xmm6, xmm4                   //xmm6 = anqz bmpt clsx dkry
		addps  xmm7, xmm6
		
		haddps xmm7, xmm7
		haddps xmm7, xmm7

		movss  xmmword ptr [l_Result], xmm7
	}
	return l_Result;
}

CMatrix4x4f& CMatrix4x4f :: Transpose()
{
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax + 48] //xmm0 = t z y x
		movaps xmm1, xmmword ptr [eax + 32] //xmm1 = s r q p
		movaps xmm2, xmmword ptr [eax + 16] //xmm2 = n m l k
		movaps xmm3, xmmword ptr [eax     ] //xmm3 = d c b a


		movaps   xmm4, xmm3                 //xmm4 = d c b a
		unpckhps xmm4, xmm1                 //xmm4 = s d r c
		movaps   xmm5, xmm4                 //xmm5 = s d r c

		movaps   xmm6, xmm2                 //xmm6 = n m l k
		unpckhps xmm6, xmm0                 //xmm6 = t n z m

		unpckhps xmm4, xmm6                 //xmm4 = t s n d
		unpcklps xmm5, xmm6                 //xmm5 = z r m c


		movaps   xmm6, xmm3                 //xmm6 = d c b a
		unpcklps xmm6, xmm1                 //xmm6 = q b p a
		movaps   xmm7, xmm6                 //xmm7 = q b p a

		unpcklps xmm2, xmm0                 //xmm2 = y l x k

		unpckhps xmm6, xmm2                 //xmm6 = y q l b
		unpcklps xmm7, xmm2                 //xmm7 = x p k a


		movaps xmmword ptr [eax     ], xmm7 // a k p x
		movaps xmmword ptr [eax + 16], xmm6 // b l q y
		movaps xmmword ptr [eax + 32], xmm5 // c m r z
		movaps xmmword ptr [eax + 48], xmm4 // d n s t
	}
	return *this;
}

CMatrix4x4f& CMatrix4x4f :: Invert()
{
    float l_Det = !(*this);
    __asm
    {
        mov eax, dword ptr this
        movaps xmm0, xmmword ptr [eax + 48] //xmm0 = t z y x
        movaps xmm1, xmmword ptr [eax + 32] //xmm1 = s r q p
        movaps xmm2, xmmword ptr [eax + 16] //xmm2 = n m l k
        movaps xmm3, xmmword ptr [eax     ] //xmm3 = d c b a


        pshufd xmm4, xmm1, 10110001b        //xmm4 = r s p q
        mulps  xmm4, xmm0                   //xmm4 = tr zs yp xq

        pshufd xmm7, xmm4, 01001110b        //xmm7 = yp xq tr zs
        mulps  xmm7, xmm2                   //xmm7 = ypn xqm trl zsk

        pshufd xmm4, xmm4, 00011011b        //xmm4 = xq yp zs tr
        mulps  xmm4, xmm2                   //xmm4 = xqn ypm zsl trk

        subps  xmm7, xmm4                   //xmm7 = ypn-xqn xqm-ypm trl-zsl zsk-trk
        pshufd xmm7, xmm7, 10110001b        //xmm7 = xqm-ypm ypn-xqn zsk-trk trl-zsl


        pshufd xmm4, xmm1, 01001110b        //xmm4 = q p s r
        mulps  xmm4, xmm0                   //xmm4 = tq zp ys xr

        pshufd xmm5, xmm4, 00011011b        //xmm5 = xr ys zp tq
        mulps  xmm5, xmm2                   //xmm5 = xrn ysm zpl tqk

        pshufd xmm4, xmm4, 10110001b        //xmm4 = zp tq xr ys
        mulps  xmm4, xmm2                   //xmm4 = zpn tqm xrl ysk

        subps  xmm5, xmm4                   //xmm5 = xrn-zpn ysm-tqm zpl-xrl tqk-ysk
        pshufd xmm5, xmm5, 01001110b        //xmm5 = zpl-xrl tqk-ysk xrn-zpn ysm-tqm
        addps  xmm7, xmm5

        pshufd xmm4, xmm1, 00011011b        //xmm4 = p q r s
        mulps  xmm4, xmm0                   //xmm4 = tp zq yr xs

        pshufd xmm5, xmm4, 10110001b        //xmm5 = zq tp xs yr
        mulps  xmm5, xmm2                   //xmm5 = zqn tpm xsl yrk

        pshufd xmm4, xmm4, 01001110b        //xmm4 = yr xs tp zq
        mulps  xmm4, xmm2                   //xmm4 = yrn xsm tpl zqk

        subps  xmm5, xmm4                   //xmm5 = zqn-yrn tpm-xsm xsl-tpl yrk-zqk
        pshufd xmm5, xmm5, 00011011b        //xmm5 = yrk-zqk xsl-tpl tpm-xsm zqn-yrn
        addps  xmm7, xmm5                   //xmm7 = D C B A



        pshufd xmm4, xmm3, 10110001b        //xmm4 = c d a b
        mulps  xmm4, xmm0                   //xmm4 = tc zd ya xb

        pshufd xmm6, xmm4, 01001110b        //xmm6 = ya xb tc zd
        mulps  xmm6, xmm1                   //xmm6 = ysa xrb tqc zpd

        pshufd xmm4, xmm4, 00011011b        //xmm4 = xb ya zd tc
        mulps  xmm4, xmm1                   //xmm4 = xsb yra zqd tpc

        subps  xmm6, xmm4                   //xmm6 = ysa-xsb xrb-yra tqc-zqd zpd-tpc
        pshufd xmm6, xmm6, 10110001b        //xmm6 = xrb-yra ysa-xsb zpd-tpc tqc-zqd


        pshufd xmm4, xmm3, 01001110b        //xmm4 = b a d c
        mulps  xmm4, xmm0                   //xmm4 = tb za yd xc

        pshufd xmm5, xmm4, 00011011b        //xmm5 = xc yd za tb
        mulps  xmm5, xmm1                   //xmm5 = xsc yrd zqa tpb

        pshufd xmm4, xmm4, 10110001b        //xmm4 = za tb xc yd
        mulps  xmm4, xmm1                   //xmm4 = zsa trb xqc ypd

        subps  xmm5, xmm4                   //xmm5 = xsc-zsa yrd-trb zqa-xqc tpb-ypd
        pshufd xmm5, xmm5, 01001110b        //xmm5 = zqa-xqc tpb-ypd xsc-zsa yrd-trb
        addps  xmm6, xmm5

        pshufd xmm4, xmm3, 00011011b        //xmm4 = a b c d
        mulps  xmm4, xmm0                   //xmm4 = ta zb yc xd

        pshufd xmm5, xmm4, 10110001b        //xmm5 = zb ta xd yc
        mulps  xmm5, xmm1                   //xmm5 = zsb tra xqd ypc

        pshufd xmm4, xmm4, 01001110b        //xmm4 = yc xd ta zb
        mulps  xmm4, xmm1                   //xmm4 = ysc xrd tqa zpb

        subps  xmm5, xmm4                   //xmm5 = zsb-ysc tra-xrd xqd-tqa ypc-zpb
        pshufd xmm5, xmm5, 00011011b        //xmm5 = ypc-zpb xqd-tqa tra-xrd zsb-ysc
        addps  xmm6, xmm5                   //xmm6 = N M L K



        pshufd xmm4, xmm2, 10110001b        //xmm4 = m n k l
        mulps  xmm4, xmm0                   //xmm4 = tm zn yk xl

        pshufd xmm5, xmm4, 01001110b        //xmm5 = yk xl tm zn
        mulps  xmm5, xmm3                   //xmm5 = ykd xlc tmb zna

        pshufd xmm4, xmm4, 00011011b        //xmm4 = xl yk zn tm
        mulps  xmm4, xmm3                   //xmm4 = xld ykc znb tma

        subps  xmm5, xmm4                   //xmm5 = ykd-xld xlc-ykc tmb-znb zna-tma
        pshufd xmm5, xmm5, 10110001b        //xmm5 = xlc-ykc ykd-xld zna-tma tmb-znb


        pshufd xmm4, xmm2, 01001110b        //xmm4 = l k n m
        mulps  xmm4, xmm0                   //xmm4 = tl zk yn xm

        pshufd xmm0, xmm4, 00011011b        //xmm0 = xm yn zk tl
        mulps  xmm0, xmm3                   //xmm0 = xmd ync zkb tla

        pshufd xmm4, xmm4, 10110001b        //xmm4 = zk tl xm yn
        mulps  xmm4, xmm3                   //xmm4 = zkd tlc xmb yna

        subps  xmm0, xmm4                   //xmm0 = xmd-zkd ync-tlc zkb-xmb tla-yna
        pshufd xmm0, xmm0, 01001110b        //xmm0 = zkb-xmb tla-yna xmd-zkd ync-tlc
        addps  xmm5, xmm0
        movaps xmm0, xmmword ptr [eax + 48] //xmm0 = t z y x 

        pshufd xmm4, xmm2, 00011011b        //xmm4 = k l m n
        mulps  xmm4, xmm0                   //xmm4 = tk zl ym xn

        pshufd xmm0, xmm4, 10110001b        //xmm0 = zl tk xn ym
        mulps  xmm0, xmm3                   //xmm0 = zld tkc xnb yma

        pshufd xmm4, xmm4, 01001110b        //xmm4 = ym xn tk zl
        mulps  xmm4, xmm3                   //xmm4 = ymd xnc tkb zla

        subps  xmm0, xmm4                   //xmm0 = zld-ymd tkc-xnc xnb-tkb yma-zla
        pshufd xmm0, xmm0, 00011011b        //xmm0 = yma-zla xnb-tkb tkc-xnc zld-ymd
        addps  xmm5, xmm0                   //xmm5 = S R Q P



		pshufd xmm0, xmm2, 10110001b        //xmm0 = m n k l
		mulps  xmm0, xmm3                   //xmm0 = md nc kb la

		pshufd xmm4, xmm0, 01001110b        //xmm4 = kb la md nc
		mulps  xmm4, xmm1                   //xmm4 = skb rla qmd pnc

		pshufd xmm0, xmm0, 00011011b        //xmm0 = la kb nc md
		mulps  xmm0, xmm1                   //xmm0 = sla rkb qnc pmd

		subps  xmm4, xmm0                   //xmm4 = skb-sla rla-rkb qmd-qnc pnc-pmd
		pshufd xmm4, xmm4, 10110001b        //xmm4 = rla-rkb skb-sla pnc-pmd qmd-qnc


		pshufd xmm0, xmm2, 01001110b        //xmm0 = l k n m
		mulps  xmm0, xmm3                   //xmm0 = ld kc nb ma

		pshufd xmm2, xmm0, 00011011b        //xmm2 = ma nb kc ld
		mulps  xmm2, xmm1                   //xmm2 = sma rnb qkc pld

		pshufd xmm0, xmm0, 10110001b        //xmm0 = kc ld ma nb
		mulps  xmm0, xmm1                   //xmm0 = skc rld qma pnb

		subps  xmm2, xmm0                   //xmm2 = sma-skc rnb-rld qkc-qma pld-pnb
		pshufd xmm2, xmm2, 01001110b        //xmm2 = qkc-qma pld-pnb sma-skc rnb-rld
		addps  xmm4, xmm2
		movaps xmm2, xmmword ptr [eax + 16] //xmm2 = n m l k

		pshufd xmm0, xmm2, 00011011b        //xmm0 = k l m n
		mulps  xmm0, xmm3                   //xmm0 = kd lc mb na

		pshufd xmm2, xmm0, 10110001b        //xmm2 = lc kd na mb
		mulps  xmm2, xmm1                   //xmm2 = slc rkd qna pmb

		pshufd xmm0, xmm0, 01001110b        //xmm0 = mb na kd lc
		mulps  xmm0, xmm1                   //xmm0 = smb rna qkd plc

		subps  xmm2, xmm0                   //xmm2 = slc-smb rkd-rna qna-qkd pmb-plc
		pshufd xmm2, xmm2, 00011011b        //xmm2 = pmb-plc qna-qkd rkd-rna slc-smb
		addps  xmm4, xmm2                   //xmm4 = T Z Y X

		//xmm4 = T Z Y X
		//xmm5 = S R Q P
		//xmm6 = N M L K
		//xmm7 = D C B A

		movaps   xmm0, xmm7                 //xmm0 = D C B A
		unpckhps xmm0, xmm5                 //xmm0 = S D R C
		movaps   xmm1, xmm0                 //xmm1 = S D R C

		movaps   xmm2, xmm6                 //xmm2 = N M L K
		unpckhps xmm2, xmm4                 //xmm2 = T N Z M

		unpckhps xmm0, xmm2                 //xmm0 = T S N D
		unpcklps xmm1, xmm2                 //xmm1 = Z R M C


		movaps   xmm2, xmm7                 //xmm2 = D C B A
		unpcklps xmm2, xmm5                 //xmm2 = Q B P A
		movaps   xmm3, xmm2                 //xmm3 = Q B P A

		unpcklps xmm6, xmm4                 //xmm6 = Y L X K

		unpckhps xmm2, xmm6                 //xmm2 = Y Q L B
		unpcklps xmm3, xmm6                 //xmm3 = X P K A

		//xmm0 = T S N D
		//xmm1 = Z R M C
		//xmm2 = Y Q L B
		//xmm3 = X P K A

		movss  xmm4, l_Det
		pshufd xmm4, xmm4, 0x0

		divps  xmm0, xmm4
		divps  xmm1, xmm4
		divps  xmm2, xmm4
		divps  xmm3, xmm4

		movaps xmmword ptr [eax     ], xmm3 // A K P X
		movaps xmmword ptr [eax + 16], xmm2 // B L Q Y
		movaps xmmword ptr [eax + 32], xmm1 // C M R Z
		movaps xmmword ptr [eax + 48], xmm0 // D N S T
    }
}

CVector4f CMatrix4x4f :: operator * (const CVector4f& a_V) const
{
	CVector4f l_Result;
	__asm
	{
		mov eax, dword ptr a_V
		movaps xmm4, xmmword ptr [eax]

		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax     ]
		movaps xmm1, xmmword ptr [eax + 16]
		movaps xmm2, xmmword ptr [eax + 32]
		movaps xmm3, xmmword ptr [eax + 48]

		mulps  xmm0, xmm4
		mulps  xmm1, xmm4
		mulps  xmm2, xmm4
		mulps  xmm3, xmm4

		haddps  xmm0, xmm1
		haddps  xmm2, xmm3
		haddps  xmm0, xmm2

		movaps xmmword ptr [l_Result], xmm0
	}
	return l_Result;
}